using UnityEngine;

namespace Kayac
{
	public class PidController3 : IPidController<Vector3>
	{
		private PidState<Vector3> state;  // PID状态存储
		private PidSettings settings;     // PID参数配置
		private bool started;             // 控制器是否已开始运行

		// 状态访问器
		public PidState<Vector3> State => state;
		public Vector3 ErrorSum
		{
			get
			{
				if (!started)
				{
					state.eSum = Vector3.zero; // 如果未开始，返回零向量
				}
				return state.eSum;
			}
			set
			{
				state.eSum = value; // 设置误差积分和
			}
		}
		public Vector3 PrevError
		{
			get => state.prevE; // 获取上一个误差
			set => state.prevE = value; // 设置上一个误差
		}

		public PidController3(PidSettings settings)
		{
			this.settings = settings;
			//Reset();
		}

		// 设置新的PID参数
		public void SetSettings(PidSettings settings)
		{
			this.settings = settings;
		}

		// 恢复保存的状态
		public void Restore(PidState<Vector3> state)
		{
			this.state = state;
			started = true; // 恢复状态后标记为已开始
		}

		// 更新PID控制器状态并返回控制输出
		public Vector3 Update(Vector3 x, Vector3 r, float deltaTime)
		{
			var f = Vector3.zero;
			// p
			var e = r - x;
			f += settings.kp * e;
			// i
			f += settings.ki * ErrorSum;
			ErrorSum += (e + PrevError) * 0.5f * deltaTime;
			// d
			if (deltaTime > 0f)
			{
				f += settings.kd * (e - PrevError) / deltaTime;
			}
			PrevError = e;
			return f;
		}

		// 重置控制器状态
		public void Reset()
		{
			started = false;
			state.prevE = Vector3.zero;
			state.eSum = Vector3.zero;
		}
	}
}